#include "Shoot_task.h"

int TEST =0;
extern remoter_t *rc;

void Shoot_Task(void *argument)
{
		Shoot_start(&UNITREE_motor_s);
		while(1)
		{
			
			switch(g_system_state.control_mode)
			{
					case MODE_NONE:
						break;	
					
					case MODE_LAUNCH:
						if(g_system_state.operate_mode == OPERATE_AUTO){
									TEST =2;
							
						}	
						else if(g_system_state.operate_mode == OPERATE_MANUAL1 || g_system_state.operate_mode == OPERATE_MANUAL2){
									Manual_launch();
									//TEST =1;
						}
						break;
					
					case MODE_ARM:
						break;
					
					default:
						break;


			}
			osDelay(5);
		}

}

void Manual_launch(void)
{
		if(g_system_state.operate_mode == OPERATE_MANUAL1)
		{
				switch (g_system_state.function_mode)
				{
					case FUNCTION_MANUAL1_OP1:
						unitree_rc_ctrl(GO_motor,&huart1,&UNITREE_motor_s,&UNITREE_motor_r,rc);
						TEST = 11;
						break;
					case FUNCTION_MANUAL1_OP2:
						TEST = 12;
						break;
					case FUNCTION_MANUAL1_OP3:
						TEST = 13;
						break;
					
					default:
						break;
				}
		
		}	
		else if(g_system_state.operate_mode == OPERATE_MANUAL2)
		{
				switch (g_system_state.function_mode)
				{
					case FUNCTION_MANUAL2_OP1:
						TEST = 21;	
						break;
					case FUNCTION_MANUAL2_OP2:
						TEST = 22;
						break;
					case FUNCTION_MANUAL2_OP3:
						TEST = 23;
						break;
					
					default:
						break;
				}
		
		}
		
}

void Shoot_start(UNITREE_MOTOR_send *unitree_motor_init)
{
	unitree_motor_init->id=pitch_motor_id;
    unitree_motor_init->K_P=0;
    unitree_motor_init->K_W=0.02;
    unitree_motor_init->mode=go_start;
    unitree_motor_init->Pos=0;
    unitree_motor_init->T=0;
	unitree_motor_init->W=0;
	unitree_uart_Send_recv(GO_motor,&huart1,unitree_motor_init,&UNITREE_motor_r);

}
